%notes: projected altitude = 229m,

%% Set Constants
g = 9.8; %m/s^2
rho = 1.2041; %kg/m^3
%Rocket Parameters:
rlen = .986; %m
rdia = .046; %m
rmass = .26; %kg
rmoment = rmass*rlen^2/12; %kg*m^2
rareat = pi*rdia^2/4; %m^2 (top area)
rareaf = rlen*rdia; %m^2 (frontal area)
farea = .046^2; %m^2 (fin area)
Cd = .08; %unitless (pressure only)
Cl = .05; %unitless (lift coefficient)

%positive means toward nose, relative to Cg
Dn = .5; %m (dist from Cg to nose)
Dp = -.3; %m (dist from Cp to Cg)
Dl = .05; %m (dist from Clift to Cg)
Df = .4; %m (dist from Cg to nose fins)

D12Curve = [0 0.049 0.116 00.184 00.237 00.282 00.297 00.311 00.322 00.348 00.386 00.442 0.546 0.718 0.879 1.066 1.257 1.436 1.590 1.612 1.650;
            0 2.569 9.369 17.275 24.258 29.730 27.010 22.589 17.990 14.126 12.099 10.808 9.876 9.306 9.105 8.901 8.698 8.310 8.294 4.613 0.000];
        
D12Curve(2, :) = D12Curve(2, :) * 1/2;

%% PrepareSim
axes = [0 20 0 230];
rel_axes = [-1 1 -2 2]*3/4;

ts = 0:.01:20;

start_theta = .15; %rad; 0 = up, pi/2 = right, -pi/2 = left
sprintf('Start angle: %.2f', start_theta);
start_omega = 0; %rad/s
start_pos = [0 0]; %[m, m]
start_vel = 0; %m/s
start_phi = 0; %rad

%% Run Sim
theta = start_theta;
omega = start_omega;
pos = start_pos;
vel = start_vel;
phi = start_phi;
psi = 0;
target = 0;
perr = 0;

pt = 0;

Fgravity = [g*rmass, pi];
Kthrust = [0, 0]; %[+thrust(t), +theta]
Kdrag = [.5*rho*Cd, pi]; %[*A*vel^2, +phi]
Klift = [.5*rho*Cl, pi/2]; %[*A*alpha*vel^2, +phi]
angles = [];
vels = [];
for t = ts
    
    dt = t - pt;
    
    alpha = wrapToPi(theta-phi);
    cosa = cos(alpha); sina = sin(alpha);
    
    Fthrust = Kthrust + [interpolate(D12Curve, t), theta];
    
    vel2 = vel^2; %speed up
    Fdrag = Kdrag .* [abs([cosa, sina])*[rareat; rareaf]*vel2, 1] + [0, phi];
    if(abs(alpha) > pi/4)
        Flift = Klift .* [0, 1] + [0, phi];
    else
        Flift = Klift .* [rareaf*alpha*vel2, 1] + [0, phi]; %may have negative magnitude
    end
    
    err = target-theta;
    psi = controlFunc(psi, err, (err-perr)/dt, 0);
    perr = err;
    beta = wrapToPi(alpha+psi);
    Ffindrag = Kdrag .* [sin(beta)*farea*vel2, 1] + [0, phi];
    if(abs(beta) > pi/3)
        Ffinlift = Klift .* [0, 1] + [0, phi];
    else
        Ffinlift = Klift .* [farea*beta*vel2, 1] + [0, phi];
    end
    
    %Fc = centripetal, Fl = linear (ie direction of phi)
    [Fcnet, Flnet, Tnet] = split(...
        convertPolarForce(Fgravity, [0 pi], phi) +...
        convertPolarForce(Fthrust, [0 theta], phi) +...
        convertPolarForce(Fdrag, [Dp theta], phi) +...
        convertPolarForce(Flift, [Dl theta], phi) +...
        convertPolarForce(Ffindrag, [Df theta], phi) +...
        convertPolarForce(Ffinlift, [Df theta], phi));
    
    vel = vel + Flnet/rmass*dt;
    if(vel < 0)
        vel = -vel;
        phi = phi + pi;
    end
    if(vel ~= 0)
        phi = wrapToPi(phi + atan(Fcnet/rmass*dt/vel));
    end
    omega = omega + Tnet/rmoment*dt;
    theta = wrapToPi(theta + omega*dt);
    
    pos = pos + vel*[sin(phi) cos(phi)];
    
    pt = t;
    
    %% plot real-time
    
    %rocket
    plot([Dn, Dn-rlen]*sin(theta)+pos(1), [Dn, Dn-rlen]*cos(theta)+pos(2));
    hold on
    plot((Dn-rlen)*sin(theta)+pos(1), (Dn-rlen)*cos(theta)+pos(2), 'x');
    plot(Dn*sin(theta)+pos(1), Dn*cos(theta)+pos(2), 'o');
    
    %forces
    Fscale = .1;
    plot([0, Fscale*Fgravity(1)*sin(Fgravity(2))]+pos(1), [0, Fscale*Fgravity(1)*cos(Fgravity(2))]+pos(2), 'r');
    plot([0, Fscale*Fthrust(1)*sin(Fthrust(2))]+pos(1), [0, Fscale*Fthrust(1)*cos(Fthrust(2))]+pos(2), 'r-');
    plot([Dp, Dp]*sin(theta)+[0, Fscale*Fdrag(1)*sin(Fdrag(2))]+pos(1), [Dp, Dp]*cos(theta)+[0, Fscale*Fdrag(1)*cos(Fdrag(2))]+pos(2), 'g');
    plot([Dl, Dl]*sin(theta)+[0, Fscale*Flift(1)*sin(Flift(2))]+pos(1), [Dl, Dl]*cos(theta)+[0, Fscale*Flift(1)*cos(Flift(2))]+pos(2), 'k');
    
    %fins
    Lfin = -.1;
    plot([Df, Df]*sin(theta)+[0, Lfin*sin(theta+psi)]+pos(1), [Df, Df]*cos(theta)+[0, Lfin*cos(theta+psi)]+pos(2), 'k');
    
    hold off
    %axis(axes)
    axis([pos(1) pos(1) pos(2) pos(2)] + rel_axes)
    drawnow
    
    pause(.05)
    
end



